#include "Detector.h"

Detection::Detection()
{
	m_width = cameraParam.resolution_width;
	m_height = cameraParam.resolution_height;
#ifdef NUC
	m_inpWidth = 448;
	m_inpHeight = 448;
#endif
#ifdef MANIFOLD_G
	m_inpWidth = 352;
	m_inpHeight = 352;
#endif
#ifdef MANIFOLD_C
	m_inpWidth = 416;
	m_inpHeight = 416;
#endif
#ifdef NX
	m_inpWidth = 480;
	m_inpHeight = 480;
#endif

	m_confThro = 0.2;
	m_NMSThro = 0.2;

	Read_Model_Armor();
}

Detection::~Detection()
{
	Dump();
}

void Detection::Dump()
{	
	m_outs.clear();
	m_boxes.clear();
	m_confs.clear();
	m_classIds.clear();
	m_perfIndx.clear();
}

void Detection::Read_Model_Armor()
{	      
    	std::string classesFile = "armor.names";
#if defined(MANIFOLD_G) || defined(NX)
    	cv::String modelConfig = "armor_gpu.cfg";
    	cv::String modelWeights = "armor_gpu.weights";
#else
   		cv::String modelConfig = "armor_cpu.cfg";
    	cv::String modelWeights = "armor_cpu.weights";
#endif	
	std::ifstream ifs(classesFile.c_str());
	std::string line;
	while (getline(ifs, line)) m_classes.push_back(line);

	model_armor = cv::dnn::readNetFromDarknet(modelConfig, modelWeights);
#ifdef NUC
	model_armor.setPreferableBackend(cv::dnn::DNN_BACKEND_INFERENCE_ENGINE);
	model_armor.setPreferableTarget(cv::dnn::DNN_TARGET_OPENCL);
#else
#if defined(MANIFOLD_G) || defined(NX)
	model_armor.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
	model_armor.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA_FP16);
#else
    model_armor.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
	model_armor.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
#endif
#endif
}

void Detection::Set_WH(int width, int height)
{
        m_width = width;
	m_height = height;
}

bool Detection::Detecting(cv::Mat frame,std::vector<cv::Rect>&box,std::vector<int> &id)
{
#ifdef DEBUG

    timespec t0, t_final;
       clock_gettime(CLOCK_MONOTONIC,&t0);
       double t = (double)cv::getTickCount();
	   
#endif


    	m_frame = frame.clone();
	cv::dnn::blobFromImage(m_frame, m_blob, 1 / 255.0,cv::Size(m_inpWidth, m_inpHeight), cv::Scalar(0, 0, 0), true, false);

	model_armor.setInput(m_blob);
	model_armor.forward(m_outs, GetOutputsNames());

	PostProcess();
		


#ifdef DEBUG
        t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
		
		
    	Drawer();
	// printf("%f\n",1.0/t);
    	cv::putText(m_frame, "fps "+std::to_string(1.0/t), cv::Point(20,50),cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0));
    	cv::imshow("CNN", m_frame);
    	// cv::waitKey(1);
#endif
        if(m_perfIndx.size()==0) return false;
        else ;
        
        for(int i=0;i<m_perfIndx.size();i++)
        {
            id.push_back(m_classIds[m_perfIndx[i]]);
            box.push_back(m_boxes[m_perfIndx[i]]);
        }
        Dump();


	return true;
}

std::vector<cv::String> Detection::GetOutputsNames()
{
	static std::vector<cv::String> names;
	if (names.empty())
	{
		std::vector<int> outLayers = model_armor.getUnconnectedOutLayers();
		std::vector<cv::String> layersNames = model_armor.getLayerNames();

		names.resize(outLayers.size());
		for (int i = 0; i < outLayers.size(); ++i)
			names[i] = layersNames[outLayers[i] - 1];
	}
	return names;
}

void Detection::PostProcess()
{
	for (int num = 0; num < m_outs.size(); num++)
	{
		cv::Point Position;
		double confidence;

		float* data = (float*)m_outs[num].data;
		for (int j = 0; j < m_outs[num].rows; j++, data += m_outs[num].cols)
		{
			cv::Mat scores = m_outs[num].row(j).colRange(5, m_outs[num].cols);
			minMaxLoc(scores, 0, &confidence, 0, &Position);
			
			if (confidence > m_confThro)
			{
				int centerX = (int)(data[0] * m_width);
				int centerY = (int)(data[1] * m_height);
				int width = (int)(data[2] * m_width);
				int height = (int)(data[3] * m_height);
				int left = centerX - width / 2;
				int top = centerY - height / 2;
				
				m_classIds.push_back(Position.x);
				m_confs.push_back((float)confidence);
				m_boxes.push_back(cv::Rect(left, top, width, height));
			}
		}
	}
	cv::dnn::NMSBoxes(m_boxes, m_confs, m_confThro, m_NMSThro, m_perfIndx);
}

void Detection::Drawer()
{
	for (int i = 0; i < m_perfIndx.size(); i++)
	{
		int idx = m_perfIndx[i];
		cv::Rect box = m_boxes[idx];
		DrawBoxes(m_classIds[idx], m_confs[idx], box.x, box.y,box.x + box.width, box.y + box.height);					
	}
}

void Detection::DrawBoxes(int classId, float conf, int left, int top, int right, int bottom)
{
	cv::rectangle(m_frame, cv::Point(left, top), cv::Point(right, bottom), cv::Scalar(255, 178, 50), 3);

	std::string label = cv::format("%.2f", conf);
	if (!m_classes.empty())
	{
		CV_Assert(classId < (int)m_classes.size());
		label = m_classes[classId] + ":" + label;
	}

	int baseLine;
	cv::Size labelSize = cv::getTextSize(label, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
	top = std::max(top, labelSize.height);
	cv::rectangle(m_frame, cv::Point(left, top - round(1.5*labelSize.height)), cv::Point(left + round(1.5*labelSize.width), top + baseLine), cv::Scalar(255, 255, 255), cv::FILLED);
	cv::putText(m_frame, label, cv::Point(left, top), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 1);
}

cv::Mat Detection::GetFrame()
{
	return m_frame;
}

int Detection::GetResWidth()
{
	return m_width;
}

int Detection::GetResHeight()
{
	return m_height;
}
